from launch import LaunchDescription
from launch_ros.actions import Node
from launch.actions import DeclareLaunchArgument, ExecuteProcess
from launch.substitutions import LaunchConfiguration
from ament_index_python.packages import get_package_share_directory
import os

def generate_launch_description():
    # 声明参数
    declare_method_arg = DeclareLaunchArgument(
        'method', 
        default_value='standard',
        description='Method to determine the topic name'
    )

    # 获取配置文件路径（修正语法）
    bridge_config_path = os.path.join(
        get_package_share_directory('control'),
        'config',
        'bridge_config.yaml'
    )
    # 创建桥接节点
    bridge_node = ExecuteProcess(
        cmd=['ros2', 'run', 'ros1_bridge', 'dynamic_bridge',
             '--bridge-config', bridge_config_path,
             '--cache-time', '1000'],
        output='screen'
    )

    return LaunchDescription([
        declare_method_arg,
        # 控制节点
        Node(
            package='control',
            executable='control',
            name='control',
            parameters=[{'method': LaunchConfiguration('method')}],
            output='screen'
        ),
        # ROS1 Bridge node
        # Node(
        #     package='ros1_bridge',
        #     executable='dynamic_bridge',
        #     name='ros1_bridge',
        #     parameters=[bridge_config_path],
        #     output='screen'
        # )
        bridge_node
        
    ])